/**
    ATClab - scriptable Air Traffic Control simulations.
    Copyright (C) 2010 openatclab@gmail.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
**/

#include "aircraft_agent.h"

#include "aircraft_data.h"
#include "aircraftsprite.h"
#include "canvas.h"

#include "logging/logger.h"

#include "performance/factory.h"
#include "performance/performance.h"

#include <cmath>
#include <qtimer.h>

#ifdef _WIN32
	#include <assert.h>
#endif

using namespace atc;
using std::make_pair;
using std::string;

////////////////////////////////////////////////////////////////////////////////
//
// atc::AircraftAgent
//
//------------------------------------------------------------------------------
// construction/destruction
//

/*!
 *
 */
AircraftAgent::AircraftAgent( QObject *parent, const char *name )
	  : QObject( parent, name )
	  , _launch( 0 )
	  , _active( true )
	  , _cfl( NAN )
	  , _vel( NAN )
	  , _data( 0 )
	  , _path( 0 )
	  , _state( 0 )
	  , _history( new AircraftHistory() )
	  , _performance( 0 )
	  , _overout_time( 0 )
	  , _handoff_time( 0 )
	  , _vectoring( false )
	  , _conflicted( false )
	  , _require_level( false )
	  , _tmp_throttle( 0.0 )
	  , _missed_acceptance( false )
	  , m_acceptanceType(ACCEPTANCE_MANUAL)
{}

/*!
 *
 */
AircraftAgent::~AircraftAgent() {
//	if ( _performance ) delete _performance;	// do not own
	if ( _history     ) delete _history;
	if ( _state       ) delete _state;
	if ( _path        ) delete _path;
	if ( _data        ) delete _data;
}

//-----------------------------------------------------------------------------
// [public] initialize
//

/*!
 * Set attributes describing aircraft
 */
void AircraftAgent::set_data( const AircraftData &data ) {
	assert( ! _data );
	_data = new AircraftData( data );
	_performance = performance::Factory::performance( data.type );
}

/*!
 * Set the initial flight path for aircraft
 */
void AircraftAgent::set_path( const AircraftPath &path ) {
	assert( ! _path );
	if ( ! path.is_valid() ) { 
		throw aircraft_agent_error( "invalid aircraft flight path" );
	}

	_path = new AircraftPath( path );

	initialize_state();
}

/*!
 *
 */
void AircraftAgent::set_launch( unsigned int ms ) {
	if ( ms ) {
		_active = false;
		_launch = ms;
	}
}


//-----------------------------------------------------------------------------
// [public] interface
//

/*!
 *
 */
void AircraftAgent::advance_position( int ms ) {
	if ( ! _active ) return;

	const AircraftState *tmp = _state;
	advance_state( ms );
	_history->add_event( tmp );
}

/*!
 *
 */
void AircraftAgent::climb( int dz ) {
	set_cfl( _cfl + dz );
}


/*!
 *
 */
bool AircraftAgent::collides_with( AircraftAgent *aa, double hnm, double vft ) {
	if ( this == aa ) return false;

	if ( ! _isnan(vft) && vft < abs( this->z() - aa->z() ) ) return false;

	if ( pow( hnm, 2 ) < pow( this->x() - aa->x(), 2 ) + pow( this->y() - aa->y(), 2 ) )
			return false;

	KeyValues kvs;
	kvs.push_back( make_pair( string("aircraft_agent"), string("conflict") ) );
	kvs.push_back( make_pair( string("second_aircraft"), aa->callsign() ) );
	kvs.push_back( make_pair( string("HDOM"), to_string(hnm) ) );
	kvs.push_back( make_pair( string("VDOM"), to_string(vft) ) );
	record_event( kvs );

	return true;
}

/*!
 *
 */
void AircraftAgent::set_conflict( bool conflicted ) {
	KeyValues kvs;
	kvs.push_back( make_pair( string("aircraft_agent"), string("set_conflict") ) );
	kvs.push_back( make_pair( string("current_conflict"), to_string(_conflicted) ) );
	kvs.push_back( make_pair( string("new_conflict"), to_string(conflicted) ) );
	record_event( kvs );

	_conflicted = conflicted;
}

/*!
 *
 */
void AircraftAgent::set_cfl( int cfl ) {
	KeyValues kvs;
	kvs.push_back( make_pair( string( "solution" ), string( "level_variation" ) ) );
	kvs.push_back( make_pair( string( "new_cfl"  ), to_string( cfl ) ) );
	kvs.push_back( make_pair( string( "old_cfl"  ), to_string( _cfl ) ) );
	record_event( kvs );

	_cfl = cfl;
	// @todo: check valid
	// @todo: invalidate path cfl's
}

/*!
 * Set the cleared velocity ( similar to cleared flight level concept )
 *
 * Velocity is dependant on altitude so that changes in altitude will effect
 * actual velocity and a specific cleared velocity may not be appropriate at 
 * a different level. The "cleared" velocity is therefore implemented in terms
 * of the throttle setting
 */
void AircraftAgent::set_velocity( int v ) {
	double old_t = _vel;
	double old_v = _state->velocity;
	double new_t = _performance->throttle_setting( z(), v );
	double new_v = v;
	double alt   = _state->z;

	_vel = new_t;
	// @todo: check valid
	// @todo: invalidate path velocities

	KeyValues kvs;
	kvs.push_back( make_pair( string( "solution"     ), string( "speed_variation" ) ) );
	kvs.push_back( make_pair( string( "new_velocity" ), to_string( new_v ) ) );
	kvs.push_back( make_pair( string( "new_throttle" ), to_string( new_t ) ) );
	kvs.push_back( make_pair( string( "old_velocity" ), to_string( old_v ) ) );
	kvs.push_back( make_pair( string( "old_throttle" ), to_string( old_t ) ) );
	kvs.push_back( make_pair( string( "altitude"     ), to_string( alt   ) ) );
	record_event( kvs );
}

/*!
 *
 */
void AircraftAgent::accept()  { 
	KeyValues kvs;
	
	ControlState current = _state->control;
	if ( CS_PROPOSED == current ) {
		set_control( CS_ACCEPTED );
		kvs.push_back( make_pair( string( "STATE_CHANGE" ), string( "accepted" ) ) );
	} else {
		kvs.push_back( make_pair( string( "STATE_CHANGE" ), string( "rejected" ) ) );
	}
	
	kvs.push_back( make_pair( string( "new_control" ), to_string( _state->control ) ) );
	kvs.push_back( make_pair( string( "old_control" ), to_string( current ) ) );
	record_event( kvs );
}

/*!
 *
 */
void AircraftAgent::handoff() { 
	KeyValues kvs;

	ControlState current = _state->control;
	if ( CS_OVEROUT == current ) {
		set_control( CS_HANDOFF );
		_handoff_time = 0;
		kvs.push_back( make_pair( string( "STATE_CHANGE" ), string( "accepted" ) ) );
	} else {
		kvs.push_back( make_pair( string( "STATE_CHANGE" ), string( "rejected" ) ) );
	}

	kvs.push_back( make_pair( string( "new_control" ), to_string( _state->control ) ) );
	kvs.push_back( make_pair( string( "old_control" ), to_string( current         ) ) );
	record_event( kvs );

}

/*!
 *
 */
void AircraftAgent::set_control( ControlState cs ){
	if ( _state->control != cs ) {
		if ( CS_OVEROUT == cs ) {
			_state->control = CS_OVEROUT;
			_overout_time = 0;
		}
		else {
			_state->control = cs;
		}
	}
}

/*!
 * Vector aircraft to new point
 */
void AircraftAgent::vector( double x, double y ) {
	vector( atan2( y - _state->y, x - _state->x ) );
}

/*!
 * Vector aircraft on new heading
 */
void AircraftAgent::vector( double a ) {
	double tmp_a = _state->heading;

	_vectoring = true;
	_state->heading = a;

	KeyValues kvs;
	kvs.push_back( make_pair( string( "solution"    ), string( "vector" ) ) );
	kvs.push_back( make_pair( string( "new_heading" ), to_string( a     ) ) );
	kvs.push_back( make_pair( string( "old_heading" ), to_string( tmp_a ) ) );
	record_event( kvs );
}

/*
 *
 */
void AircraftAgent::reroute( const AircraftPathToken *t, Point p ) {
	double tmp_a = _state->heading;

	_vectoring = false;
	_path->reroute( t, p, position() );
	_state->heading = atan2( p.second - y(), p.first - x() );

	KeyValues kvs;
	kvs.push_back( make_pair( string( "solution"    ), string( "reroute" ) ) );
	kvs.push_back( make_pair( string( "new_heading" ), to_string( _state->heading ) ) );
	kvs.push_back( make_pair( string( "old_heading" ), to_string( tmp_a           ) ) );
	kvs.push_back( make_pair( string( "xcoord"      ), to_string( p.first         ) ) );
	kvs.push_back( make_pair( string( "ycoord"      ), to_string( p.second        ) ) );
	record_event( kvs );
}

/*!
 *
 */
void AircraftAgent::require_level( int z ) {
	// @todo: to simple ??

	KeyValues kvs;
	kvs.push_back( make_pair( string( "solution"     ), string( "requirement") ) );
	kvs.push_back( make_pair( string( "new_level"    ), to_string( z    ) ) );
	kvs.push_back( make_pair( string( "old_level"    ), to_string( _cfl ) ) );
	kvs.push_back( make_pair( string( "old_throttle" ), to_string( _state->throttle ) ) );
	record_event( kvs );

	set_cfl( z );
	_require_level = true;
	_tmp_throttle = _vel;
	_vel = 1.0;				// expediate
}

/*!
 *
 */
double AircraftAgent::speed() const {
	return _state->velocity;
}

/*!
 *
 */
Point AircraftAgent::position( int ms ) {
	// current position
	if ( !ms ) 
		return Point( x(), y() );

	double s = ms * _state->velocity / ONE_HOUR;

	// follow vector
	if ( _vectoring )
		return Point( x() + s * cos( a() ), y() + s * sin( a() ) );

	// position on path
	return _path->future_point( Point( x(), y() ), s );
}

/*!
 *
 */
const char * AircraftAgent::callsign() const { return _data->call; }
const char * AircraftAgent::type    () const { return _data->type; }


/*!
 *
 */
double AircraftAgent::calt() const { return _cfl; }

ControlState AircraftAgent::control() const { return _state->control; }
double       AircraftAgent::x()       const { return _state->x; }
double       AircraftAgent::y()       const { return _state->y; }
double       AircraftAgent::z()       const { return _state->z; }
double       AircraftAgent::a()       const { return _state->heading; }

/*!
 *
 */
double AircraftAgent::vel_min() const { 
	return _performance->cruise_speed_minimum( z() );
}
double AircraftAgent::vel_std() const { 
	return _performance->cruise_speed_standard( z() );
}
double AircraftAgent::vel_max() const { 
	return _performance->cruise_speed_maximum( z() );
}

/*!
 *
 */
double AircraftAgent::cfl_min() const { 
	return 0;
}
double AircraftAgent::cfl_std() const { 
	return _performance->cruising_altitude();
}
double AircraftAgent::cfl_max() const { 
	return _performance->ceiling();
}

//-----------------------------------------------------------------------------
// [private] slots
//

/*!
 * 
 */
void AircraftAgent::activate() {
	_active = true;
	QTimer::singleShot( 0, this, SIGNAL( activated() ) );
	update_state();
}

/*!
 * 
 */
void AircraftAgent::deactivate() {
    _active = false;
	QTimer::singleShot( 0, this, SIGNAL( deactivated() ) );
}

//-----------------------------------------------------------------------------
// [private]
//

/*!
 * 
 */
bool AircraftAgent::has_path() {
	return _path && _path->is_valid();
}

/*!
 * 
 */
void AircraftAgent::initialize_state() {
	if ( ! has_path() ) 
		throw aircraft_agent_error( "aircraft's flight path not defined" );
	if ( _state ) 
		throw aircraft_agent_error( "aircraft already initialized" );

	const AircraftPathToken *path = _path->data();

	bool is_level         = false;
	bool not_accelerating = false;

	//
	// set
	//
	long         tt = 0;
	ControlState cs = CS_NONE;

	//
	// set position
	//
	double x = path->x0;
	double y = path->y0;

	//
	// set heading
	//
	double  b = atan2( path->y1 - y ,  path->x1 - x );

	//
	// set altitude
	//
	double z = _isnan( path->z0 ) ? _performance->cruising_altitude() 
			 :                      path->z0
			 ;

	//
	// set cruising velocity
	//
	double vel = _isnan( path->v0 ) ? _performance->cruise_speed_standard( z )
		       :                      path->v0
		       ;

	//
	// update throttle setting
	//
	double t = _performance->throttle_setting( z, vel );

	//
	// set cleared flight level
	//
	if ( _isnan( path->z1 ) ) {
		_cfl = z;
		is_level = true;
	}
	else {
		_cfl = path->z1;
		is_level = ( int(_cfl) == int(z) );
	}

	//
	// set cleared velocity
	//
	if ( _isnan( path->v1 ) ) {
		_vel = t;
		not_accelerating = true;
	}
	else {
		_vel = _performance->throttle_setting( z, path->v1 );
	}

	//
	// update airspeed
	//
	double v = ( is_level ) ?  vel
	         : ( _cfl > z ) ?  _performance->climb_speed( z, t )
	         :                 _performance->climb_speed( z, t )
	         ;

	//
	// update climb rate
	//
	double c = ( is_level ) ?  0.0
	         : ( _cfl > z ) ?  _performance->climb_rate( z, t )
	         :                -_performance->descent_rate( z, t )
	         ;

	//
	// update acceleration
	//
	double a = ( not_accelerating ) ? 0.0
	         :                        _performance->acceleration( z, t )
	         ;

	//
	// new state
	//
	_state = new AircraftState( tt, cs, x, y, z, b, v, a, c, t );

	update_state();
}

/*!
 * 
 */
void AircraftAgent::advance_state( int ms ) {
	assert( _state );

	bool is_level         = false;
	bool not_accelerating = false;

	// current state (update time tag)
	long         tag = _state->tag + ms;
	ControlState  cs = _state->control;
	double         x = _state->x;
	double         y = _state->y;
	double         z = _state->z;
	double         b = _state->heading;
	double         v = _state->velocity;
	double         a = _state->acceleration;
	double         c = _state->climb_rate;
	double         t = _state->throttle;


	//
	// @todo: automatic handoff of aircraft could be scripted
	// @todo: if 'handoff key' is set disable auto handoff
	// @todo: otherwise on.
	//

	// advance Controller status if 'over out'
	if ( CS_OVEROUT == cs ) {
		_overout_time += ms;
		if ( _overout_time > CS_OVEROUT_PERIOD ) { 
			cs = CS_HANDOFF;
			handoff();
		}
	}

	// advance Controller status if 'hand off'
	else if ( CS_HANDOFF == cs ) {
		_handoff_time += ms;
		if ( _handoff_time > CS_HANDOFF_PERIOD ) cs = CS_NOMORE;
	}

	// current position on flight path
	const AircraftPathToken *path = _path->data();

	// update period in seconds
	double period = (double)ms / ONE_SECOND;
	
	//
	// calc position
	//
	double deltaX = period * v * cos( b ) / SIXTY_SQR;	// knots -> nM this period
	double deltaY = period * v * sin( b ) / SIXTY_SQR;	//

	double tdist = hypot( path->x1 - x, path->y1 - y );
	double ddist = hypot( deltaX, deltaY );

	if ( ddist >= tdist ) {
		ddist -= tdist;
		_path->next();
		QTimer::singleShot( 0, this, SIGNAL( waypoint() ) );

		if ( _path->done() ) {
			deactivate();
			_state = new AircraftState( *_state );
			return;
		}
		else {
			path = _path->data();
			
			b = atan2( path->y1 - path->y0, path->x1 - path->x0 );

			x = path->x0 + ddist * cos( b );
			y = path->y0 + ddist * sin( b );

			if ( ! _isnan( path->z1 ) ) { _cfl = path->z1; }
			if ( ! _isnan( path->v1 ) ) { 
				_vel = _performance->throttle_setting( z, path->v1 ); 
			}
		}
	}
	else {
		x += deltaX;
		y += deltaY;
	}

	//
	// calc altitude
	//
	double deltaZ = period * c / SIXTY;		// feet/min -> feet this period

	if ( abs( _cfl - z ) <= abs( deltaZ ) ) { 
		z = _cfl;
		is_level = true;

		if ( _require_level ) {
			_require_level = false;
			_vel = _tmp_throttle;
		}

	}
	else { 
		z += deltaZ; 
	}

	//
	// calc cruising velocity
	// 
	double deltaV = period * a / SIXTY;		// Knt/min -> Knts this period
	double    vel = _performance->cruise_speed( z, t );
	double   tvel = _performance->cruise_speed( z, _vel );

	if ( abs( tvel - vel ) <= abs( deltaV ) ) { 
		vel = tvel;    
		not_accelerating = true;
		t = _vel;
	}
	else {	
		vel += deltaV; 
		t = _performance->throttle_setting( z, vel );
	}

	//
	// update airspeed
	//
	v = ( is_level ) ?  vel
	  : ( _cfl > z ) ?  _performance->climb_speed( z, t )
	  :                 _performance->climb_speed( z, t )
	  ;

	//
	// update climb rate
	//
	c = ( is_level ) ?  0.0
	  : ( _cfl > z ) ?  _performance->climb_rate( z, t )
	  :                -_performance->descent_rate( z, t )
	  ;

	//
	// update acceleration
	//
	a = ( not_accelerating ) ? 0.0
	  : ( _vel > t         ) ? _performance->acceleration( z, t )
	  :                        -_performance->deceleration( z, t )
	  ;

	//
	// new state
	//
	_state = new AircraftState( tag, cs, x, y, z, b, v, a, c, t );
	update_state();
}

/*!
 *
 */
void AircraftAgent::update_state() {
	QTimer::singleShot( 0, this, SIGNAL( updated() ) );

	KeyValues kvs;
	kvs.push_back( make_pair( string( "type"    ), to_string( _data->type       ) ) );
	kvs.push_back( make_pair( string( "control" ), to_string( _state->control    ) ) );
	kvs.push_back( make_pair( string( "xpos"    ), to_string( _state->x          ) ) );
	kvs.push_back( make_pair( string( "ypos"    ), to_string( _state->y          ) ) );
	kvs.push_back( make_pair( string( "alt"     ), to_string( _state->z          ) ) );
	kvs.push_back( make_pair( string( "vel"     ), to_string( _state->velocity   ) ) );
	kvs.push_back( make_pair( string( "head"    ), to_string( _state->heading    ) ) );
	kvs.push_back( make_pair( string( "climb"   ), to_string( _state->climb_rate ) ) );
	kvs.push_back( make_pair( string( "power"   ), to_string( _state->throttle   ) ) );
	record_event( kvs );
}

/*!
 *
 */
void AircraftAgent::record_event( KeyValues &kvs ) {
	kvs.push_front( make_pair( string( "call" ), callsign() ) );
	emit event_recorded( kvs );
}


////////////////////////////////////////////////////////////////////////////////
